pi = 4*atan(1);

A=[
277.0  -142.0  -458.0  -159.0   -15.0   852.0;
124.0   -63.0  -801.0  -305.0    -3.0  1046.0;
67.0   -35.0  -856.0  -337.0   -53.0  1126.0;
119.0   -50.0  -691.0  -269.0   -61.7   975.9;
247.0  -135.0  -789.0  -345.0   -46.0   792.0;
183.0   -72.0  -948.0  -450.0    20.4  1089.0;
-99.0    47.0   664.0   406.9   -34.0  -851.0;
-311.0   176.0   519.0   343.0   -30.0  -574.0;
-348.0   199.4   543.0   268.0    39.0  -554.0;
-71.0    40.0   522.0   318.0    17.0  -698.0;
-101.0    60.7   534.3   300.3   -15.0  -647.0;
-129.0    72.0   542.0   343.3    27.0  -474.5;
];

B=[
-490.0  -242.0   -15.0   916.0   547.0  -321.0;
-584.0  -334.0   -38.0   949.0   387.0  -190.0;
-657.0  -404.0     2.0  1241.0   646.0  -281.0;
-809.0  -487.0   -14.0  1109.0   793.0  -451.0;
-735.0  -443.0    -8.0  1275.0   798.0  -451.0;
-763.0  -470.0    12.0  1167.0   675.1  -324.8;
528.0   312.0     1.0  -814.0  -612.0   347.0;
353.0   194.0    10.0  -868.0  -594.0   366.1;
282.0   173.0    -8.0  -634.0  -493.0   291.0;
491.0   283.0    21.0  -723.0  -492.0   322.0;
432.0   265.0   -47.0  -961.0  -674.0   432.0;
366.0   229.0   -62.0  -483.0  -385.5   249.0;
];

C=[
18.0   646.0   912.0  -525.0  -792.0  -237.1;
21.0   654.0   440.9  -251.0  -596.0  -109.0;
28.0   375.0   782.0  -398.0  -938.0  -228.0;
51.0   648.0   908.0  -471.6  -716.0  -259.5;
14.0   150.0   781.0  -371.0  -919.0  -191.2;
21.0   414.0   738.0  -380.0  -718.5  -301.9;
-23.0  -390.0  -513.0   325.0   369.4   243.6;
-10.0  -277.0  -567.3   421.0   519.3   230.9;
7.0   -75.0  -585.0   322.0   407.0   257.0;
-39.0  -511.0  -686.8   405.4   343.8   239.6;
-20.0  -564.0  -654.0   382.0   647.0   346.0;
-19.0  -221.0  -416.0   348.0   294.0   190.8;
];

R=[
27.0  -588.0    19.0 -1047.0    54.0  -840.0;
26.0 -1098.0    74.0 -1893.0   143.0 -1297.0;
49.0 -1744.0   134.0 -2221.0   148.0 -2165.0;
153.0 -2820.0    85.0 -4064.0   317.0 -2483.0;
147.0 -2889.0   123.0 -5082.0   252.0 -3951.0;
73.0 -2514.0   203.0 -6283.0   360.0 -5076.0;
-31.0   732.0   -48.0  1065.0   -56.0   898.0;
-34.0  1190.0   -72.0  2024.0   -84.0  1790.3;
-20.0  1324.0  -111.0  3099.0  -211.0  2585.0;
-78.0  2746.0  -115.0  4226.0  -384.0  3150.0;
-146.0  4123.1   -80.0  5156.0  -378.0  3625.0;
-124.0  3287.0  -290.0  5833.0  -426.0  4666.0;
];

//A
iA=[
0.9 0 0;
1.1 0 0;
2.0 0 0;
1.2 0 0;
1.0 0 0;
1.3 0 0;
-0.95 0 0;
-0.725 0 0;
-0.85 0 0;
-0.9 0 0;
-0.8 0 0;
-0.75 0 0;
]*10; //cm->mm 0 0;

//B (+2pi/3)
iB=[
0.95 0 0;
1.25 0 0;
1.30 0 0;
1.20 0 0;
1.35 0 0;
1.35 0 0;
-1.00 0 0;
-0.95 0 0;
-0.8 0 0;
-0.8 0 0;
-1.0 0 0;
-0.65 0 0;
]*10; //cm->mm

theta=-2*pi/3;
irB=iB*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

//C (+4pi/3)
iC=[
1.3 0 0;
1.1 0 0;
1.1 0 0;
1.3 0 0;
1.2 0 0;
1.3 0 0;
-0.85 0 0;
-0.9 0 0;
-0.8 0 0;
-0.9 0 0;
-0.85 0 0;
-0.7 0 0;
]*10; //cm->mm

theta=-4*pi/3;
irC=iC*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

//R
iR=[
0 0 6;
0 0 13;
0 0 16;
0 0 22;
0 0 27;
0 0 35;
0 0 -4;
0 0 -10;
0 0 -19;
0 0 -24;
0 0 -26;
0 0 -31;
]*pi/180;

mult = 2^14;

Moutput = [A(:,3:6);B(:,3:6);C(:,3:6);R(:,3:6)];
Minput = [iA;irB;irC;iR]*mult;

[arc,la,lb,sig,resid] = armax( 0, 0, Moutput', Minput');

[Ax,Bx,Dx]=arma2p(arc);   //Results in polynomial form. 

M = pinv(coeff(Bx));

// show matrix
M

u = mopen('matrix.h','w')

fprintf(u,'double hrobot_adnsMatrix[3][6] = {\n');
for j=1:3 
  fprintf(u,'{');
  for i=1:4
    fprintf(u,'%f',M(j,i));
    if(i ~= 4),
      fprintf(u,',');
    end
  end
  fprintf(u,'}');
  if(j ~= 3),
    fprintf(u,',');
  end
  fprintf(u,'\n');
end
fprintf(u,'};\n');

mclose(u);
